#include "Trajectory.h"



PointTraj::PointTraj(long double latitude, long double longitude, long double altitude, long double timestamp)
{
	this->latitude = latitude;
	this->longitude = longitude;
	this->altitude = altitude;
	this->timestamp = timestamp;

}
PointTraj::~PointTraj(void){

}




Trajectory::Trajectory(const char* file)
{
	   ifstream pFile;
	   char buffer[255];
	   char* splitedString;
	   long double latitud;
	   long double longitud;
	   long double altitud;
	   long double timestam;



	   //open a file and read it : works
	   
	   pFile.open(file);
	   if (!pFile.is_open()) perror ("Error opening file");
	   else
	   {
		 
			int i;
			for(i=0;i<6;i++){
				//skip the 6 first lines
				if(!pFile.eof())
					pFile.getline(buffer, 255);
			}
			PointTraj p = PointTraj(0,0,0,0);
			PointTraj pOld = PointTraj(0,0,0,0);
			int j=7;
			while( !pFile.eof())
			{
				//we count to only get the right informations
				i=0;

				pFile.getline(buffer, 255);
				splitedString = strtok(buffer, ",");
				while (splitedString != NULL)
				  {

					//printf ("%d : %s\n",i,splitedString);
					if(i == 0)
						latitud = atof(splitedString);
					if(i == 1)
						longitud = atof(splitedString);
					if(i == 3)
						altitud = atof(splitedString);
					if(i == 4)
						timestam = atof(splitedString);				

					i++;
					//repeat the call until the end
					splitedString = strtok (NULL, ",");
				  }
				

				//if not the first point
				if(p.getTimestamp() != 0){
					pOld = p;
				}
				//create the point
				p = PointTraj(latitud, longitud, altitud, timestam);

				//if not the first point
				if(p.getTimestamp() != 0){
					//calculate distance
					//double dist = distance2Points(pOld, p);

					//make interpolation and add the point to the vector if no need
					interpolation2Points(&points, pOld, p);
					//cout << "interpol\n";
					//cout << "dist "<< j <<" : " << dist << "\n";
				}
				
				j++;
				//add it to the vector
				//addPointTraj(p);
				
			}
			
			//printDistPoints();

			 pFile.close();
			

			 
	   }

}


Trajectory::~Trajectory(void)
{
}

void Trajectory::addPointTraj(PointTraj p)
{
		this->points.push_back(p);
}

//calculate distance between 2 points
long double Trajectory::distance2Points(PointTraj p1, PointTraj p2){
	
	
	
	//y
	long double lat1 = p1.getLatitude();
	long double lat2 = p2.getLatitude();

	//x
	long double longi1 = p1.getLongitude();
	long double longi2 = p2.getLongitude();

	// to get the length or the arc: distArc = angle*R*Pi/180
	long double a = M_PI/180;

	//angle difference * earth radius in m
	long double distLat = (lat2 - lat1)*6371000*a;
	long double distLongi = (longi2 - longi1)*6371000*a;


	//distance between the 2 points
	long double dist = sqrt(distLongi*distLongi + distLat*distLat);

	return dist;

}


PointTraj Trajectory::makeMiddlePoint(PointTraj p1, PointTraj p2){

	long double latMiddle = (p1.getLatitude()+p2.getLatitude())/2;
	long double longMiddle = (p1.getLongitude()+p2.getLongitude())/2;
	long double altMiddle = (p1.getAltitude()+p2.getAltitude())/2;
	long double timeMiddle = (p1.getTimestamp()+p2.getTimestamp())/2;

	PointTraj middlePoint = PointTraj(latMiddle, longMiddle, altMiddle, timeMiddle);

	//temporary point to allow compile if other works
	return middlePoint;
}


//recursive interpolation, stop case is distance < D then add to the vector
void Trajectory::interpolation2Points(vector<PointTraj> *vect, PointTraj p1, PointTraj p2){
	
	//if first point
	if(p1.getTimestamp() == 0){
		
		//add p2 to vector
		(*vect).push_back(p2);

	}
	else{
		if(distance2Points(p1, p2) > 100){
		
		
			//make middle point
			PointTraj middlePoint = makeMiddlePoint(p1, p2);
			/*
			cout << "p1 :\n lat : "<< p1.getLatitude() << "\nlong : " << p1.getLongitude() << "\n";

			cout << "p2 :\n lat : "<< p2.getLatitude() << "\nlong : " << p2.getLongitude() << "\n";

			cout << "pmid :\n lat : "<< middlePoint.getLatitude() << "\nlong : " << middlePoint.getLongitude() << "\n";

			cout << "dist 1 2 : " << distance2Points(p1, p2) << "\n";

			cout << "dist 1 Mid : " << distance2Points(p1, middlePoint) << "\n";
			
			cout << "dist Mid 2 : " << distance2Points(middlePoint, p2) << "\n";

			system("pause");
			*/

			//interpol p1, middle point 
			interpolation2Points(vect, p1, middlePoint);
			//interpol middle point, p2
			interpolation2Points(vect, middlePoint, p2);

		}else{
			//add p2 to vector
			(*vect).push_back(p2);
		}
	}

}



void Trajectory::printDistPoints(){

	cout << "print Function\n";
	//print all the distances from the vector
	PointTraj pTemp = PointTraj(0,0,0,0);
	int j=7;
	for(vector<PointTraj>::iterator i = points.begin(); i != points.end(); i++){
		//if not the first point
	if((*i).getTimestamp() != 0){
					
		long double dist = distance2Points(pTemp, (*i));
		cout << "dist "<< j <<" : " << dist << "\n";

	}
	j++;
	pTemp = (*i);
	}

}